#include "direct_navigation/direct_navigator.hpp"
#include <jsoncpp/json/json.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <rclcpp_lifecycle/lifecycle_publisher.hpp>
#include <lifecycle_msgs/msg/transition.hpp>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include <deque>
#include <vector>
#include <numeric>

namespace direct_navigation {

    DirectNavigator::DirectNavigator(const rclcpp::NodeOptions & options)
        : rclcpp_lifecycle::LifecycleNode("direct_navigator", options)
    {
        RCLCPP_INFO(this->get_logger(), "Lifecycle Direct Navigator is now created.");
        // 不再在此处初始化，因为生命周期节点的初始化由状态机驱动
    }

    // 声明参数
    void DirectNavigator::declare_parameters(){
        center_json_topic_ = this->declare_parameter("center_json_topic", "/neutron_center_json");
        cmd_vel_topic_ = this->declare_parameter("cmd_vel_topic", "/cmd_vel");
        reached_topic_ = this->declare_parameter("reached_topic", "/navigation_reached");

        max_linear_velocity_ = this->declare_parameter("max_linear_velocity", 0.1);
        y_position_tolerance_ = this->declare_parameter("y_position_tolerance", 0.01);
        x_position_tolerance_ = this->declare_parameter("x_position_tolerance", 0.02);
        controlling_period_ = this->declare_parameter("controlling_period", 200);
        max_acceleration_ = this->declare_parameter("max_acceleration", 0.1);
        max_angular_acceleration_ = this->declare_parameter("max_angular_acceleration", 0.1);

        double sampling_duration_sec = this->declare_parameter("sampling_duration", 3.0);  // 秒
        sampling_duration_ = std::chrono::duration<double>(sampling_duration_sec);

        SIGMA_THRESHOLD_ = this->declare_parameter("sigma_threshold", 3.0);
    }

    CallbackReturn DirectNavigator::on_configure(const rclcpp_lifecycle::State & state) 
    {
        RCLCPP_INFO(this->get_logger(), "Configuring...");

        // 1. 声明参数
        declare_parameters();
        // 2. 初始化成员变量 (除了 ROS 接口相关)
        current_center_x_ = 0.0;
        current_center_y_ = 0.0;
        current_center_z_ = 0.0;
        target_detected_ = false;
        control_state_ = IDLE;
        avg_center_x_ = 0.0;
        avg_center_y_ = 0.0;
        avg_center_z_ = 0.0;
        sampling_start_time_ = rclcpp::Time(0, 0, this->get_clock()->get_clock_type()); // 初始化为0
        // sampling_duration_ 已在 declare_parameters 中设置
        prev_linear_x_ = 0.0;
        prev_linear_y_ = 0.0;
        prev_angular_z_ = 0.0;
        linear_x = 0.0;
        linear_y = 0.0;
        angular_z = 0.0;
        target_linear_x = 0.0;
        target_linear_y = 0.0;
        target_angular_z = 0.0;
        movement_timer_ = 0.0;
        movement_time_x_ = 0.0;
        movement_time_y_ = 0.0;
        state_is_active_ = false; // 初始为非活动状态

        sample_points_.clear(); // 清空样本点

        RCLCPP_INFO(this->get_logger(), "Successfully configured.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }


    CallbackReturn DirectNavigator::on_activate(const rclcpp_lifecycle::State & state)
    {
        RCLCPP_INFO(this->get_logger(), "Activating...");

        // 创建发布者和订阅者
        center_json_sub_ = this->create_subscription<std_msgs::msg::String>(
            center_json_topic_, 10,
            std::bind(&DirectNavigator::centerJsonCallback, this, std::placeholders::_1));

        cmd_vel_pub_ = this->create_publisher<std_msgs::msg::String>(cmd_vel_topic_, 10);
        reached_pub_ = this->create_publisher<std_msgs::msg::Bool>(
            reached_topic_, rclcpp::QoS(1).transient_local());
        
        // 创建控制循环定时器 
        control_timer_ = this->create_wall_timer(
            std::chrono::milliseconds(controlling_period_),
            std::bind(&DirectNavigator::controlLoop, this));

        // 激活发布者 (LifecyclePublisher 需要显式激活)
        cmd_vel_pub_->on_activate();
        reached_pub_->on_activate();

        // 重置状态为 IDLE
        control_state_ = IDLE;
        state_is_active_ = true; // 标记为活动状态

        RCLCPP_INFO(this->get_logger(), "Successfully activated.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    } 
    
    CallbackReturn DirectNavigator::on_deactivate(const rclcpp_lifecycle::State & state)
    {
        RCLCPP_INFO(this->get_logger(), "Deactivating...");

        // 停用发布者
        if (cmd_vel_pub_) cmd_vel_pub_->on_deactivate();
        if (reached_pub_) reached_pub_->on_deactivate();

        // 取消定时器
        if (control_timer_) {
            control_timer_->cancel();
        }

        // 停止机器人
        stopRobot();

        state_is_active_ = false; // 标记为非活动状态

        RCLCPP_INFO(this->get_logger(), "Successfully deactivated.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }


    CallbackReturn DirectNavigator::on_cleanup(const rclcpp_lifecycle::State & state)
    {
        RCLCPP_INFO(this->get_logger(), "Cleaning up...");

    
        // 1. 重置状态标志
        state_is_active_ = false;
        // 2. 重置所有内部状态变量
        current_center_x_ = 0.0;
        current_center_y_ = 0.0;
        current_center_z_ = 0.0;
        target_detected_ = false;
        control_state_ = IDLE;
        avg_center_x_ = 0.0;
        avg_center_y_ = 0.0;
        avg_center_z_ = 0.0;
        sampling_start_time_ = rclcpp::Time(0, 0, this->get_clock()->get_clock_type());
        // sampling_duration_ 保持不变，因为它来自参数
        prev_linear_x_ = 0.0;
        prev_linear_y_ = 0.0;
        prev_angular_z_ = 0.0;
        linear_x = 0.0;
        linear_y = 0.0;
        angular_z = 0.0;
        target_linear_x = 0.0;
        target_linear_y = 0.0;
        target_angular_z = 0.0;
        movement_timer_ = 0.0;
        movement_time_x_ = 0.0;
        movement_time_y_ = 0.0;

        sample_points_.clear(); // 清空样本点

        // 3. 重置 ROS 接口指针
        center_json_sub_.reset();
        cmd_vel_pub_.reset();
        reached_pub_.reset();
        control_timer_.reset();

        RCLCPP_INFO(this->get_logger(), "Successfully cleaned up.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
        
    }


    void DirectNavigator::centerJsonCallback(const std_msgs::msg::String::SharedPtr msg)
    {
        // 关键修改：如果节点未激活，则不处理数据
        if (!state_is_active_) {
            return;
        }

        if(center_coords.parse_json_string(msg->data))
        {
            current_center_x_ = center_coords.get_coord_x();
            current_center_y_ = center_coords.get_coord_y();
            current_center_z_ = center_coords.get_coord_z();
            target_detected_ = true;
        }
        else
        {
            target_detected_ = false;
        }
    }

    void DirectNavigator::controlLoop()
    {
        // 关键修改：如果节点未激活，则不执行控制循环
        if (!state_is_active_) {
            return;
        }

        switch(control_state_) {
            case IDLE:
                handleIdleState();
                break;
            case SAMPLING_Y:
                handleSamplingYState();
                break;
            case MOVING_Y:
                handleMovingYState();
                break;
            case SAMPLING_X:
                handleSamplingXState();
                break;
            case MOVING_X:
                handleMovingXState();
                break;
            //case VERIFYING:
                //handleVerifyingState();
                //break;
            case REACHED:
                handleReachedState();
                break;
        }
    }

    void DirectNavigator::handleIdleState()
    {
        if (!target_detected_) {
            stopRobot();
            return;
        }

        // 开始第一次取样
        startSampling();
        control_state_ = SAMPLING_Y;
        RCLCPP_INFO(this->get_logger(), "当正处于IDLE状态, 开始进行静止采样");
    }

    void DirectNavigator::handleSamplingYState()
    {
        if (!target_detected_) {
            stopRobot();
            control_state_ = IDLE;
            return;
        }

        // 记录第一个样本的时间
        if (sample_points_.empty()) {
            sampling_start_time_ = this->now();
        }

        // 添加当前数据到样本
        sample_points_.push_back({current_center_x_, current_center_y_, current_center_z_});

        // 检查是否收集足够时间的样本
        auto current_time = this->now();
        auto elapsed_time = (current_time - sampling_start_time_).seconds();

        if (elapsed_time >= sampling_duration_.count()) {
            processSamples();
            sample_points_.clear();
            
            // 根据y轴距离判断控制方式
            if (std::abs(avg_center_y_) > y_position_tolerance_) {
                // Y轴需要调整
                control_state_ = MOVING_Y;
                startMovingYToTarget();
                RCLCPP_INFO(this->get_logger(), "状态切换: SAMPLING_Y → MOVING_Y (distance=%.3f)", 
                           std::abs(avg_center_y_));
            } else {
                // Y轴达标，开始x轴判断
                control_state_ = SAMPLING_X;
                startSampling();
                RCLCPP_INFO(this->get_logger(), "状态切换: SAMPLING_Y → SAMPLING_X");
            }
        }
    }

    
    void DirectNavigator::handleMovingYState()
    {
        // 类似x轴控制
        double dt = static_cast<double>(controlling_period_) / 1000.0;
        movement_timer_ += dt;
        
        if (movement_timer_ < movement_time_y_) {
            applyVelocitySmoothing(linear_x, linear_y, angular_z, 0.0, target_linear_y, 0.0);
            publishVelocity();
        } else {
            stopRobot();
            movement_timer_ = 0.0;
            // Y轴移动完成后，进入验证状态
            control_state_ = SAMPLING_X;
            startSampling();
            RCLCPP_INFO(this->get_logger(), "移动完成，状态切换: MOVING_Y → SAMPLING_X");
        }
        RCLCPP_INFO(this->get_logger(),"速度为: lineax: %.3f, lineary: %.3f, angularz: %.3f !", linear_x, linear_y, angular_z);
    }


    void DirectNavigator::handleSamplingXState()
    {
        if (!target_detected_) {
            stopRobot();
            control_state_ = IDLE;
            return;
        }

        // 记录第一个样本的时间
        if (sample_points_.empty()) {
            sampling_start_time_ = this->now();
        }

        // 添加当前数据到样本
        sample_points_.push_back({current_center_x_, current_center_y_, current_center_z_});

        // 检查是否收集足够时间的样本
        auto current_time = this->now();
        auto elapsed_time = (current_time - sampling_start_time_).seconds();

        if (elapsed_time >= sampling_duration_.count()) {
            processSamples();
            sample_points_.clear();
            
            // 根据x轴距离判断控制方式
            if (std::abs(avg_center_x_) > x_position_tolerance_) {
                // x > tolerance: 控制x
                control_state_ = MOVING_X;
                startMovingToTarget();
                RCLCPP_INFO(this->get_logger(), "状态切换: SAMPLING_X → MOVING_X (distance=%.3f)", 
                           std::abs(avg_center_x_));
            } else {
                // x < tolerance: 开始取样验证
                control_state_ = REACHED;
                // startSampling();  // 开始取样
                RCLCPP_INFO(this->get_logger(), "状态切换: SAMPLING_Y → REACHED");
            }
        }
    }

    void DirectNavigator::handleMovingXState()
    {
        // 发布固定速度，基于计算的距离和速度
        double dt = static_cast<double>(controlling_period_) / 1000.0;
        movement_timer_ += dt;
        
        if (movement_timer_ < movement_time_x_) {
            // 发布固定速度
            applyVelocitySmoothing(linear_x, linear_y, angular_z, target_linear_x, target_linear_y, 0.0);
            publishVelocity();
        } else {
            // 运动时间到，停止并进入验证
            stopRobot();
            movement_timer_ = 0.0;
            // X轴移动完成后，直接进入Y轴采样
            control_state_ = REACHED;
            startSampling();
            RCLCPP_INFO(this->get_logger(), "状态切换: MOVING_X  → REACHED");
        }
        RCLCPP_INFO(this->get_logger(),"速度为: lineax: %.3f, lineary: %.3f, angularz: %.3f !", linear_x, linear_y, angular_z);
    }



    /* void DirectNavigator::handleVerifyingState()
    {
        if (!target_detected_) {
            stopRobot();
            control_state_ = IDLE;
            return;
        }

         // 记录第一个样本的时间
        if (sample_points_.empty()) {
            sampling_start_time_ = this->now();
        }

        // 添加当前数据到样本
        sample_points_.push_back({current_center_x_, current_center_y_, current_center_z_});

        auto current_time = this->now();
        auto elapsed_time = (current_time - sampling_start_time_).seconds();

        // 检查是否收集足够时间的样本
        if (elapsed_time >= sampling_duration_.count()) {
            processSamples();
            sample_points_.clear();
            
            bool x_ok = (std::abs(avg_center_x_) <= x_position_tolerance_);
            bool y_ok = (std::abs(avg_center_y_) <= y_position_tolerance_);

            if(x_ok && y_ok){
                // 完全到达目标
                control_state_ = REACHED;
                publishReachedStatus(true);  // 传递成功状态
                RCLCPP_INFO(this->get_logger(), "TARGET REACHED! x=%.3f, y=%.3f", 
                           avg_center_x_, avg_center_y_);
            } else {
                
                // 开始第一次取样
                startSampling();

                if(!x_ok){
                    control_state_ = SAMPLING_X;
                    RCLCPP_WARN(this->get_logger(), "Verification failed (X=%.3f > %.3f)", 
                               std::abs(avg_center_x_), x_position_tolerance_);
                } else{
                    
                    startSampling();
                    control_state_ = SAMPLING_Y;
                    RCLCPP_WARN(this->get_logger(), "Verification failed (Y=%.3f > %.3f)", 
                               std::abs(avg_center_y_), y_position_tolerance_);
                }
            }

        }
    } */

   
     void DirectNavigator::handleReachedState()
    {
        // 精细对准模式：当放射源在中心附近时
        if (!target_detected_) {
            RCLCPP_WARN(this->get_logger(), "Target lost in REACHED state!");
            control_state_ = IDLE;
            stopRobot();
            return;
        }

        stopRobot();
        publishReachedStatus(true);
    }


    void DirectNavigator::startSampling()
    {
        sample_points_.clear();
        sampling_start_time_ = this->now();
        stopRobot();  // 停止机器人开始取样
    }

    void DirectNavigator::processSamples()
    {
        if (sample_points_.size() < 3) {
            // 样本太少，使用当前值
            avg_center_x_ = current_center_x_;
            avg_center_y_ = current_center_y_;
            return;
        }

        // 去除离群值（使用3σ原则）
        std::vector<double> x_coords, y_coords;
        for (const auto& point : sample_points_) {
            x_coords.push_back(point.x);
            y_coords.push_back(point.y);
        }

        // 计算均值和标准差
        auto [filtered_x, filtered_y] = removeOutliers(x_coords, y_coords);

        // 计算平均值
        avg_center_x_ = std::accumulate(filtered_x.begin(), filtered_x.end(), 0.0) / filtered_x.size();
        avg_center_y_ = std::accumulate(filtered_y.begin(), filtered_y.end(), 0.0) / filtered_y.size();

        RCLCPP_INFO(this->get_logger(), 
                   "Processed samples: avg_x=%.3f, avg_y=%.3f (original samples: %zu, filtered: %zu)",
                   avg_center_x_, avg_center_y_, sample_points_.size(), filtered_x.size());
    }

    std::pair<std::vector<double>, std::vector<double>> 
    DirectNavigator::removeOutliers(const std::vector<double>& x_coords, const std::vector<double>& y_coords)
    {
        if (x_coords.size() < 3) return {x_coords, y_coords};

        // 计算x坐标均值和标准差
        double x_mean = std::accumulate(x_coords.begin(), x_coords.end(), 0.0) / x_coords.size();
        double x_var = 0.0;
        for (double x : x_coords) {
            x_var += (x - x_mean) * (x - x_mean);
        }
        x_var /= x_coords.size();
        double x_std = std::sqrt(x_var);

        // 计算y坐标均值和标准差
        double y_mean = std::accumulate(y_coords.begin(), y_coords.end(), 0.0) / y_coords.size();
        double y_var = 0.0;
        for (double y : y_coords) {
            y_var += (y - y_mean) * (y - y_mean);
        }
        y_var /= y_coords.size();
        double y_std = std::sqrt(y_var);

        // 2σ原则（而不是3σ）

        // 去除离群值
        std::vector<double> filtered_x, filtered_y;
        for (size_t i = 0; i < x_coords.size(); ++i) {
            if (std::abs(x_coords[i] - x_mean) <= SIGMA_THRESHOLD_ * x_std && 
                std::abs(y_coords[i] - y_mean) <= SIGMA_THRESHOLD_ * y_std) {
                filtered_x.push_back(x_coords[i]);
                filtered_y.push_back(y_coords[i]);
            }
        }

        return {filtered_x, filtered_y};
    }

    void DirectNavigator::startMovingToTarget()
    {
        // 计算运动参数
        double distance_x = std::abs(avg_center_x_ - x_position_tolerance_);
        //double distance_y = std::abs(avg_center_y_);
        //double total_distance = std::sqrt(distance_x * distance_x + distance_y * distance_y);

        if (distance_x < 1e-6) {
            // 距离很近，直接停止
            movement_time_x_ = 0.0;
            movement_time_y_ = 0.0;
            target_linear_x = 0.0;
            target_linear_y = 0.0;
            return;
        }

         // 计算最大运动时间（以最长轴为准）
        // double max_distance = std::max(distance_x, distance_y);
        // 计算运动时间（包含加速减速）
        double max_movement_time = calculateMovementTime(distance_x, max_linear_velocity_, max_acceleration_);

        movement_time_x_ = max_movement_time;  // 两轴同时到达
        //movement_time_y_ = max_movement_time;
        movement_timer_ = 0.0;

        // 按比例分解速度
        //double ratio_x = distance_x / total_distance;
        //double ratio_y = distance_y / total_distance;
        
        // 计算目标速度（方向）
        target_linear_x = max_linear_velocity_ * (avg_center_x_ > 0 ? 1 : -1);
        //target_linear_y = max_linear_velocity_ * ratio_y * (avg_center_y_ > 0 ? 1 : -1);

        
        RCLCPP_INFO(this->get_logger(), 
                   "Moving to target: distance_x=%.3f, time_x=%.2f",
                   distance_x, movement_time_x_);
    }

    void DirectNavigator::startMovingYToTarget()
    {
        double distance_y = std::abs(avg_center_y_);

        movement_time_y_ = calculateMovementTime(distance_y, max_linear_velocity_, max_acceleration_);
        target_linear_y = max_linear_velocity_ * (avg_center_y_ > 0 ? 1 : -1);
        movement_timer_ = 0.0;
        
        RCLCPP_INFO(this->get_logger(), "Moving Y to target: distance_y=%.3f, time=%.2f", 
                   distance_y, movement_time_y_);
    }

    void DirectNavigator::stopRobot()
    {
        applyVelocitySmoothing(linear_x, linear_y, angular_z, 0.0, 0.0, 0.0);
        publishVelocity();
    }

    void DirectNavigator::publishVelocity()
    {
        vel_cmd.set_from_twist(linear_x, linear_y, angular_z);
        std::string json_string = vel_cmd.get_json_string();

        if(!json_string.empty() && json_string != "null") {
            std_msgs::msg::String msg;
            msg.data = json_string;
            cmd_vel_pub_->publish(msg);
        }
    }

    // 运动时间计算函数
    double DirectNavigator::calculateMovementTime(double distance, double max_vel, double max_acc)
    {
        if (distance < 1e-6) return 0.0;

        // 计算加速到最大速度所需的时间和距离
        double time_to_max_vel = max_vel / max_acc;
        double distance_to_max_vel = 0.5 * max_acc * time_to_max_vel * time_to_max_vel;
        
        //RCLCPP_INFO(this->get_logger(),"最大加速时间：%.3f, 最大加速距离：%.3f",time_to_max_vel, distance_to_max_vel);

        // 如果距离足够让机器人达到最大速度（梯形模式）
        if (distance >= 2.0 * distance_to_max_vel) {
            // 梯形模式：加速 -> 匀速 -> 减速
            double constant_vel_distance = distance - 2.0 * distance_to_max_vel;
            double constant_vel_time = constant_vel_distance / max_vel;

            RCLCPP_INFO(this->get_logger(),"实际移动时间：%.3f", 2.0 * time_to_max_vel + constant_vel_time);
            
            return 2.0 * time_to_max_vel + constant_vel_time;

        } else {
            // 三角形模式：加速 -> 减速（达不到最大速度）
            // distance = 0.5 * max_acc * t_acc^2 + 0.5 * max_acc * t_dec^2
            // 由于对称，t_acc = t_dec，所以 distance = max_acc * t_acc^2
            double accel_time = std::sqrt(distance / max_acc);

            RCLCPP_INFO(this->get_logger(),"实际移动时间：%.3f",2.0 * accel_time);
            
            return 2.0 * accel_time;

        }
    }


    // 速度平滑函数
    void DirectNavigator::applyVelocitySmoothing(double& linear_x, double& linear_y, double& angular_z,
                                            double target_linear_x, double target_linear_y, double target_angular_z) {
        
        double dt = static_cast<double>(controlling_period_) / 1000.0;
        
        double max_linear_change = max_acceleration_ * dt;
        double max_angular_change = max_angular_acceleration_ * dt;
        
        double dx_change = std::clamp(target_linear_x - prev_linear_x_, 
                                    -max_linear_change, max_linear_change);
        double dy_change = std::clamp(target_linear_y - prev_linear_y_, 
                                    -max_linear_change, max_linear_change);
        double dtheta_change = std::clamp(target_angular_z - prev_angular_z_, 
                                        -max_angular_change, max_angular_change);
                                        
        linear_x = prev_linear_x_ + dx_change;
        linear_y = prev_linear_y_ + dy_change;
        angular_z = prev_angular_z_ + dtheta_change;

        prev_linear_x_ = linear_x;
        prev_linear_y_ = linear_y;
        prev_angular_z_ = angular_z;

        RCLCPP_INFO(this->get_logger(), "dt=%.3f, max_linear_change=%.3f, dx_change=%.3f, dy_change=%.3f", 
                    dt, max_linear_change, dx_change, dy_change);
        RCLCPP_INFO(this->get_logger(), "prev_linear_x_=%.3f, prev_linear_y_=%.3f, prev_angular_z_=%.3f", 
                    prev_linear_x_, prev_linear_y_, prev_angular_z_);
    }


    void DirectNavigator::publishReachedStatus(bool success)
    {
        std_msgs::msg::Bool reached_msg;
        reached_msg.data = success;  // 正确的bool赋值
        
        if (success) {
            reached_pub_->publish(reached_msg);
            RCLCPP_INFO(this->get_logger(), "Published REACHED status: success=%s", 
                       success ? "true" : "false");
        } else {
            RCLCPP_WARN(this->get_logger(), "Published REACHED status: success=false");
        }
    }

} // namespace direct_navigation

// --- 主函数修改 ---
int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);

    // 创建生命周期节点
    auto node = std::make_shared<direct_navigation::DirectNavigator>(rclcpp::NodeOptions());

    // 使用 MultiThreadedExecutor 以便正确处理生命周期
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node->get_node_base_interface());
    executor.spin();

    rclcpp::shutdown();
    return 0;
}